# -*- coding: utf-8 -*-
#!/usr/bin/env python

import rospy
from quadrotor_msgs.msg import PositionCommand
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Pose
from mavros_msgs.msg import PositionTarget
from std_msgs.msg import Bool

# UAV数量
uav_id = $id  

cmd_pub1 = None
cmd_pub2 = None

# uint16 type_mask
# uint16 IGNORE_PX = 1 # Position ignore flags
# uint16 IGNORE_PY = 2
# uint16 IGNORE_PZ = 4
# uint16 IGNORE_VX = 8 # Velocity vector ignore flags
# uint16 IGNORE_VY = 16
# uint16 IGNORE_VZ = 32
# uint16 IGNORE_AFX = 64 # Acceleration/Force vector ignore flags
# uint16 IGNORE_AFY = 128
# uint16 IGNORE_AFZ = 256
# uint16 FORCE = 512 # Force in af vector flag
# uint16 IGNORE_YAW = 1024
# uint16 IGNORE_YAW_RATE = 2048

# 回调函数处理PositionCommand并发布Pose

local_pose = [None]*3
fucking_stop = False
fucking_stop_racer = False
now_stop_pose = [None]*3
yaw = 0.0
yaw_rate = 0.0

def position_command_callback(data):
    global fucking_stop, fucking_stop_racer, yaw, yaw_rate
    # 创建新的Pose消息

    if not fucking_stop:
        if local_pose[2] <= -3.0:
            fucking_stop = True
            command_msg2 = PositionTarget()
            command_msg2.velocity.x = 0
            command_msg2.velocity.y = 0
            command_msg2.velocity.z = 0
            #速度控制
            command_msg2.type_mask=0b111111000111
            cmd_pub2.publish(command_msg2)
        else:
            #位置控制
            if fucking_stop_racer:
                return
            else:
                command_msg1 = Pose()
                command_msg1.position.x = data.position.x
                command_msg1.position.y = data.position.y
                command_msg1.position.z = data.position.z
                if abs(data.yaw) > 4.0:
                    command_msg1.orientation.x = yaw
                else:
                    command_msg1.orientation.x = data.yaw
                    yaw = data.yaw
                if abs(data.yaw_dot) > 2.0:
                    command_msg1.orientation.y = yaw_rate
                else:
                    command_msg1.orientation.y = data.yaw_dot
                    yaw_rate = data.yaw_dot
                cmd_pub1.publish(command_msg1)
    else:
        command_msg3 = PositionTarget()
        command_msg3.velocity.x = 0
        command_msg3.velocity.y = 0
        command_msg3.velocity.z = 0
        #速度控制
        command_msg3.type_mask=0b111111000111
        cmd_pub2.publish(command_msg3)

    

def position_command_callback2(msg):
    global fucking_stop_racer
    local_pose[0] = msg.pose.position.x
    local_pose[1] = msg.pose.position.y
    local_pose[2] = msg.pose.position.z

    if fucking_stop_racer:
        command_msg1 = Pose()
        command_msg1.position.x = now_stop_pose[0]
        command_msg1.position.y = now_stop_pose[1]
        command_msg1.position.z = now_stop_pose[2]
        cmd_pub1.publish(command_msg1)



def position_command_callback3(msg):
    global fucking_stop_racer
    fucking_stop_racer = msg.data
    if fucking_stop_racer:
        now_stop_pose[0] = local_pose[0]
        now_stop_pose[1] = local_pose[1]
        now_stop_pose[2] = local_pose[2]


if __name__ == '__main__':
    # 初始化ROS节点
    rospy.init_node('racer_command_to_px4_'+str(uav_id))

    # 订阅每个UAV的PositionCommand话题
    sub_topic = '/planning/pos_cmd_{}'.format(uav_id + 1)
    sub_topic2 = '/iris_{}/mavros/local_position/pose'.format(uav_id)
    sub_topic3 = '/fucking_stop_{}'.format(uav_id + 1)

    rospy.Subscriber(sub_topic, PositionCommand, position_command_callback ,queue_size=10)
    rospy.Subscriber(sub_topic2, PoseStamped, position_command_callback2 ,queue_size=10)
    rospy.Subscriber(sub_topic3, Bool, position_command_callback3 ,queue_size=10)

    # 创建发布Pose消息的Publisher
    pub_topic = '/iris_{}/mavros/setpoint_raw/local'.format(uav_id) 
    cmd_pub2= rospy.Publisher(pub_topic, PositionTarget, queue_size=10)

    pub_topic = '/xtdrone/iris_{}/cmd_pose_enu'.format(uav_id) 
    cmd_pub1= rospy.Publisher(pub_topic, Pose, queue_size=10)
    # 保持节点运行
    rospy.spin()